/* Copyright (c) 2009 Mark L. Howison
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *  (1) Redistributions of source code must retain the above copyright notice,
 *      this list of conditions and the following disclaimer.
 *  (2) Redistributions in binary form must reproduce the above copyright
 *      notice, this list of conditions and the following disclaimer in the
 *      documentation and/or other materials provided with the distribution.
 *  (3) The name of the copyright holder may not be used to endorse or promote
 *      products derived from this software without specific prior written
 *      permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDER AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

package edu.berkeley.edrl.wiikinemathics;

import java.util.Comparator;
import java.util.TreeSet;
import javax.vecmath.Vector2d;
import wiiremotej.IRLight;
import wiiremotej.event.WRIREvent;
import wiiremotej.event.WRStatusEvent;
import wiiremotej.event.WiiRemoteAdapter;

class IRLightComparator implements Comparator<IRLight>
{
    public int compare(IRLight l1, IRLight l2) {
        if (l1.getX() < l2.getX()) return -1;
        else if (l1.getX() > l2.getX()) return 1;
        else return 0;
    }
}

/**
 *
 * @author mhowison
 */
public class MotionTracker extends WiiRemoteAdapter {

    private final static double GAMETRAK_ADJUSTMENT = 2.0;
    
    private final static char STATE_IDLE      = 0;
    private final static char STATE_CALIBRATE = 1;
    private final static char STATE_TRACKING  = 2;

    private final static double TARGET_HZ = 60.0;

    private boolean calibrated = false;
    private char state = STATE_IDLE;
    private double trackerHeight;
    private Vector2d leftStart = new Vector2d();
    private Vector2d rightStart = new Vector2d();
    private Vector2d axis = new Vector2d();
    private TreeSet<IRLight> orderedLights = new TreeSet(new IRLightComparator());
    
    public MotionTracker() {
    }

    public void calibrate() {
        calibrated = false;
        state = STATE_CALIBRATE;
    }

    public boolean isCalibrated() {
        return calibrated;
    }

    public void track() {
        trackerHeight = 10.0 / WiiKinemathicsApp.getControls().heightSlider.getValue();
        if (WiiKinemathicsApp.INPUT_DEVICE == WiiKinemathicsApp.INPUT_WIIMOTE) {
            trackerHeight /= WiiKinemathicsApp.WIICAM_HEIGHT;
        } else {
            trackerHeight *= trackerHeight;
        }
        state = STATE_TRACKING;
    }

    public void idle() {
        calibrated = false;
        state = STATE_IDLE;
    }

    private void transformLights(IRLight[] lights) {
        for (int i=0; i<lights.length; i++) {
            if (lights[i] != null) {
                lights[i] = new IRLight(
                        (int)(lights[i].getY() * WiiKinemathicsApp.WIICAM_WIDTH),
                        (int)(lights[i].getX() * WiiKinemathicsApp.WIICAM_HEIGHT),
                        (int)(lights[i].getSize() * WiiKinemathicsApp.WIICAM_DOTSIZE));
            }
        }
    }

    @Override
    public void IRInputReceived(WRIREvent event) {
        IRLight[] lights = event.getIRLights();
        transformLights(lights);
        orderedLights.clear();
        WiiKinemathicsApp.getApplication().cameraPanel.update(lights);
        for (int i=0; i<lights.length; i++) {
            if (lights[i] != null) {
                orderedLights.add(lights[i]);
            }
        }
        if (orderedLights.size() < 2) {
            //app.getLogger().warning("Found < 2 IR dots.");
        } else if (orderedLights.size() > 2) {
            //app.getLogger().warning("Found > 2 IR dots.");
        }
        else {
            IRLight right  = orderedLights.last();
            IRLight left = orderedLights.first();
            //app.getLogger().info(String.format("found dots: %f %f %f %f", left.getX(), left.getY(), right.getX(), right.getY()));
            if (state == STATE_CALIBRATE) {
                leftStart.set(
                        left.getX()*WiiKinemathicsApp.WIICAM_WIDTH,
                        left.getY()*WiiKinemathicsApp.WIICAM_HEIGHT);
                rightStart.set(
                        right.getX()*WiiKinemathicsApp.WIICAM_WIDTH,
                        right.getY()*WiiKinemathicsApp.WIICAM_HEIGHT);
                axis.set(rightStart);
                axis.sub(leftStart);
                axis.set(-axis.y, axis.x);
                axis.normalize();
                calibrated = true;
            }
            else if (state == STATE_TRACKING) {
                double leftHeight, rightHeight;
                Vector2d position = new Vector2d();
                position.set(
                        left.getX()*WiiKinemathicsApp.WIICAM_WIDTH,
                        left.getY()*WiiKinemathicsApp.WIICAM_HEIGHT);
                position.sub(leftStart);
                leftHeight = position.dot(axis) * trackerHeight;
                position.set(
                        right.getX()*WiiKinemathicsApp.WIICAM_WIDTH,
                        right.getY()*WiiKinemathicsApp.WIICAM_HEIGHT);
                position.sub(rightStart);
                rightHeight = position.dot(axis) * trackerHeight;
                WiiKinemathicsApp.getApplication().motionPanel.updateTracker(leftHeight, rightHeight);
            }
        }
    }

    public void gameTrak(byte[] buf) {
        StringBuilder coords[] = new StringBuilder[2];
        coords[0] = new StringBuilder(16);
        coords[1] = new StringBuilder(16);
        int j = 0;
        for (int i=0; i<buf.length; i++) {
            char c = (char)buf[i];
            if (c == ' ') j++;
            else if (c == ';') break;
            else coords[j].append(c);
        }
        double z0 = Double.valueOf(coords[0].toString());
        double z1 = Double.valueOf(coords[1].toString());
        if (state == STATE_CALIBRATE) {
            leftStart.x = z0;
            rightStart.x = z1;
            calibrated = true;
        } else if (state == STATE_TRACKING) {
            z0 = GAMETRAK_ADJUSTMENT * (leftStart.x - z0) * trackerHeight;
            z1 = GAMETRAK_ADJUSTMENT * (rightStart.x - z1) * trackerHeight;
            WiiKinemathicsApp.getApplication().motionPanel.updateTracker(z0,z1);
        }
    }

    @Override
    public void statusReported(WRStatusEvent e) {
        WiiKinemathicsApp.getApplication().updateBattery(e.getSource(), e.getBatteryLevel());
    }

    @Override
    public void disconnected() {
        WiiKinemathicsApp.getApplication().connect();
    }
}
